optim.stw <- function(a, ref, samp, time, B, P, try = FALSE) {

## Optimization routine

nb <- ncol(B)

if (!try) {
  for (it in 1:20) {

    w <- time + t(B %*% a)
    IP <- interpol.stw(w, samp)
    
    # Improve
    r <- ref[IP$sel] - IP$z
    msr <- sqrt(mean(r^2))
    G <- IP$g * matrix(1, length(IP$g), nb)
    Q <- G * B[IP$sel, ]
    a <- solve((t(Q) %*% Q + P), (t(Q) %*% (t(r) + Q %*% a)))
    cat("it: ", it, "   msr: ", msr, "\n")
  }
} else {
  w <- time + t(B %*% a)
  IP <- interpol.stw(as.numeric(w), as.numeric(samp))
  r <- ref[IP$sel] - IP$z
  msr <- sqrt(mean(r^2))
}
  
  list(par = a, value = msr)

}
